#ifndef LIDAR_LOCALIZATION_MODELS_CERES_OPTIMIZER_HPP_
#define LIDAR_LOCALIZATION_MODELS_CERES_OPTIMIZER_HPP_

#include <ceres/ceres.h>
#include <Eigen/Dense>
#include <sophus/se3.hpp>
#include <sophus/so3.hpp>
#include <vector>
#include "lidar_localization/models/graph_optimizer/interface_graph_optimizer.hpp"
namespace lidar_localization
{
//定义顶点以及边的数据结构 用一个类来管理所有的数据
typedef Eigen::Matrix<double,6,1> Vector6d;
typedef Eigen::Matrix<double,6,6> Matrix6d;
struct cereNode
{
    /* data */
    unsigned int index;
    double se3[6] = {0};
    bool need_fix = false;
};
struct cereEdge
{
    unsigned int index1;
    unsigned int index2;
    Sophus::SE3d se3;
    Matrix6d conversie;
};

struct cereEdgeXYZ
{
    unsigned int index;
    Eigen::Vector3d trans;
    Eigen::Matrix3d conversie;
};

Matrix6d JRInv(const Sophus::SE3d &e);
// class SE3Parameterization : public ceres::LocalParameterization
// {
// public:
//     SE3Parameterization() {}
//     virtual ~SE3Parameterization() {}

//     virtual bool Plus(const double* x,
//                       const double* delta,
//                       double* x_plus_delta) const
//     {
//         Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie(x);
//         Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_lie(delta);

//         Sophus::SE3d T = Sophus::SE3d::exp(lie);
//         Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_lie);

//         // 李代数左乘更新
//         Eigen::Matrix<double, 6, 1> x_plus_delta_lie = (delta_T * T).log();

//         for(int i = 0; i < 6; ++i)
//             x_plus_delta[i] = x_plus_delta_lie(i, 0);

//         return true;
//     }
//     virtual bool ComputeJacobian(const double* x,
//                                  double* jacobian) const
//     {
//         ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
//         return true;
//     }
//     virtual int GlobalSize() const { return 6; }
//     virtual int LocalSize() const { return 6; }
// };
class PoseGraphCostFunction : public ceres::SizedCostFunction<6,6,6>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual ~PoseGraphCostFunction(){}
    PoseGraphCostFunction(Sophus::SE3d _se3, Matrix6d _covariance): measurment_se3(_se3), convariance(_covariance){}

    virtual bool Evaluate(double const* const* parameters,
                          double *residuals,
                          double **jacobians) const{

        //Create SE3 with parameters
        Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
        Sophus::SE3d pose_j = Sophus::SE3d::exp(Vector6d(parameters[1]));
        Sophus::SE3d estimate = pose_i.inverse() * pose_j;

        //Get information matrix by LLT decomposition
        // Matrix6d sqrt_info = Eigen::LLT<Matrix6d>(convariance.inverse()).matrixLLT().transpose();
        Matrix6d sqrt_info = convariance;
        Eigen::Map<Vector6d> residual(residuals);
//        residual = sqrt_info * ((measurment_se3.inverse() * estimate).log());
        residual = (measurment_se3.inverse() * estimate).log();

        //Compute jacobians matrix
        if(jacobians){
            if(jacobians[0]) {
                Eigen::Map<Matrix6d> jacobian_i(jacobians[0]);
                Matrix6d J = JRInv(Sophus::SE3d::exp(residual));
                jacobian_i = (sqrt_info * (-J) * pose_j.inverse().Adj()).transpose();//雅各比矩阵的表达公式 J^T * info_matrix * J => J' = sqrt_info * J
            }

            if(jacobians[1]){
                Eigen::Map<Matrix6d> jacobian_j(jacobians[1]);
                Matrix6d J = JRInv(Sophus::SE3d::exp(residual));
                jacobian_j = (sqrt_info * J * pose_j.inverse().Adj()).transpose();
            }
        }
        residual = sqrt_info * ((measurment_se3.inverse() * estimate).log());//整体误差的表达公式 l(x) = e_ij^T * covariance * e_ij， 当delta_x -> 0的时候，l(x) -> 0
        return true;
    }
private:
    const Sophus::SE3d measurment_se3;
    const Matrix6d convariance;
};
void Convert2se3(Eigen::Isometry3d &mesent, Sophus::SE3d &se3);


class SE3Parameterization : public ceres::LocalParameterization
{
public:
    SE3Parameterization() {}
    virtual ~SE3Parameterization() {}

    virtual bool Plus(const double* x,
                      const double* delta,
                      double* x_plus_delta) const
    {
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> lie(x);
        Eigen::Map<const Eigen::Matrix<double, 6, 1>> delta_lie(delta);

        Sophus::SE3d T = Sophus::SE3d::exp(lie);
        Sophus::SE3d delta_T = Sophus::SE3d::exp(delta_lie);

        // 李代数左乘更新
        Eigen::Matrix<double, 6, 1> x_plus_delta_lie = (delta_T * T).log();

        for(int i = 0; i < 6; ++i)
            x_plus_delta[i] = x_plus_delta_lie(i, 0);

        return true;
    }
    virtual bool ComputeJacobian(const double* x,
                                 double* jacobian) const
    {
        ceres::MatrixRef(jacobian, 6, 6) = ceres::Matrix::Identity(6, 6);
        return true;
    }
    virtual int GlobalSize() const { return 6; }
    virtual int LocalSize() const { return 6; }
};


class PoseGraphCostFunctionXYZ : public ceres::SizedCostFunction<3,6>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;

    virtual ~PoseGraphCostFunctionXYZ(){}
    PoseGraphCostFunctionXYZ(Eigen::Vector3d _se3, Eigen::Matrix3d _covariance): measurment_se3(_se3), convariance(_covariance){}

    virtual bool Evaluate(double const* const* parameters,
                          double *residuals,
                          double **jacobians) const{

        //Create SE3 with parameters
        Sophus::SE3d pose_i = Sophus::SE3d::exp(Vector6d(parameters[0]));
        // Eigen::Matrix4d gnsspose =  pose_i.matrix();
        


        Eigen::Vector3d pose_i_trans = pose_i.matrix().block<3,1>(0,3);



        // Eigen::Quaterniond q(gnsspose.block<3,3>(0,0));
        

        // Sophus::SE3d gnss_se3(q.normalized(),measurment_se3);
        //Get information matrix by LLT decomposition
        // Eigen::Matrix3d sqrt_info = Eigen::LLT<Eigen::Matrix3d>(convariance.inverse()).matrixLLT().transpose();
 
        Eigen::Matrix3d sqrt_info = convariance;
        // Eigen::Map<Eigen::Vector3d> residual(residuals);
//        residual = sqrt_info * ((measurment_se3.inverse() * emstiate).log());
        Eigen::Vector3d residual = (measurment_se3 - pose_i_trans);
        residuals[0] = residual.x();
        residuals[1] = residual.y();
        residuals[2] = residual.z();


        //Compute jacobians matrix\\\\






        if(jacobians){
            if(jacobians[0]) {
                Eigen::Map<Eigen::Matrix<double,3,6>,Eigen::RowMajor> jacobian_i(jacobians[0]);
                jacobian_i.setZero();
                jacobian_i.block<3,3>(0,3) = -1  * Eigen::Matrix3d::Identity();
            }
        }
        return true;
    }
private:
    const Eigen::Vector3d measurment_se3;
    const Eigen::Matrix3d convariance;
};










class CeresGraphOptimizer {
public:
    CeresGraphOptimizer(){};
    // 优化
    bool Optimize() ;
    // 输入、输出数据
    bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) ;
    int GetNodeNum() ;
    // 添加节点、边、鲁棒核
    // void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size){}
    void AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) ;
    void AddSe3Edge(int vertex_index1,
                            int vertex_index2,
                            const Eigen::Isometry3d &relative_pose,
                            Vector6d noise);
    void AddSe3PriorXYZEdge(int se3_vertex_index,
                                    const Eigen::Vector3d &xyz,
                                    Eigen::Vector3d noise);
    void AddSe3PriorQuaternionEdge(int se3_vertex_index,
                                           const Eigen::Quaterniond &quat,
                                           Eigen::VectorXd noise);
    // 设置优化参数
    void SetMaxIterationsNum(int max_iterations_num);
private:
    std::vector<cereNode> nodeDataBuff;
    std::vector<cereEdge> edgeDataBuff;
    std::vector<cereEdgeXYZ> edgeXYZDatabuff;
};
}








#endif